/******************************************************************************
 *
 * MotionViewer
 * Copyright (C) 2007 Sangyoon Lee, Electronic Visualization Laboratory, 
 * University of Illinois at Chicago
 *
 * This software is free software; you can redistribute it and/or modify it 
 * under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation; either Version 2.1 of the License, or 
 * (at your option) any later version.
 *
 * This software is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 
 * License for more details.
 * 
 * You should have received a copy of the GNU Lesser Public License along
 * with this software; if not, write to the Free Software Foundation, Inc., 
 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 *
 * Questions or comments about MotionViewer should be directed to 
 * cavern@evl.uic.edu
 *
 *****************************************************************************/
#include <ogreplatform.h>
#if OGRE_PLATFORM == OGRE_PLATFORM_APPLE
#include <Carbon/Carbon.h>
#endif


#include "ViconRTVisualizer.h"
#include "ViconRTData.h"


ViconRTVisualizer::ViconRTVisualizer(SceneManager* mgr, SceneNode* head)
				 : ViconVisualizer(mgr, head)
{
}

ViconRTVisualizer::~ViconRTVisualizer()
{
	m_vMarkerNodes.clear();

	for(int i=0; i<m_iBodies; i++)
	{
		delete m_vBodies[i];
	}

	m_vBodies.clear();

	if (m_pDataSet)
	{
		delete m_pDataSet;
		m_pDataSet = NULL;
	}
}

void ViconRTVisualizer::createScene(char* source)
{
	m_pDataSet = new ViconRTData();
	m_pDataSet->connectToSource(source);
	m_iMarkers = m_pDataSet->getNumMarkers();
	m_iBodies = m_pDataSet->getNumBodies();

	// create markers
	char namestr[40];
	Entity* emarker;
	m_vMarkerNodes.resize(m_iMarkers);
	for(int i=0; i<m_iMarkers; i++)
	{
		sprintf(namestr, "marker_%i_%s", m_iUID, m_pDataSet->getMarkerName(i).c_str());
		emarker = m_pSceneMgr->createEntity(namestr, Ogre::SceneManager::PT_SPHERE);
		emarker->setMaterialName(m_pMarkerMaterial->getName());
		m_vMarkerNodes[i] = m_pBaseNode->createChildSceneNode(namestr);
		m_vMarkerNodes[i]->scale(Ogre::Vector3(0.02, 0.02, 0.02));
		m_vMarkerNodes[i]->attachObject(emarker);
	}

	// create bodies
	Entity* ebody;
	m_vBodies.resize(m_iBodies);
	sVICONBODY* pBody;
	SceneNode* pSNode;
	ManualObject* pMNode;
	for(int i=0; i<m_iBodies; i++)
	{
		// body
		sprintf(namestr, "body_%i_%s", m_iUID, m_pDataSet->getBodyName(i).c_str());
		ebody = m_pSceneMgr->createEntity(namestr, Ogre::SceneManager::PT_CUBE);
		ebody->setMaterialName(m_pBodyMaterial->getName());
		pSNode = m_pBaseNode->createChildSceneNode(namestr);
		pSNode->scale(Ogre::Vector3(0.04, 0.04, 0.04));
		pSNode->attachObject(ebody);

		// axis
		sprintf(namestr, "axis_%i_%s", m_iUID, m_pDataSet->getBodyName(i).c_str());
		pMNode =  m_pSceneMgr->createManualObject(namestr);
		pMNode->begin("mocapRedMaterial", Ogre::RenderOperation::OT_LINE_LIST);
		pMNode->position(0, 0, 0); 
		pMNode->position(250, 0, 0);
		pMNode->end(); 
		pMNode->begin("mocapGreenMaterial", Ogre::RenderOperation::OT_LINE_LIST);
		pMNode->position(0, 0, 0); 
		pMNode->position(0, 250, 0);
		pMNode->end(); 
		pMNode->begin("mocapBlueMaterial", Ogre::RenderOperation::OT_LINE_LIST);
		pMNode->position(0, 0, 0); 
		pMNode->position(0, 0, 250);
		pMNode->end(); 
		pSNode->attachObject(pMNode);
		
		pBody = new sVICONBODY;
		pBody->pNode = pSNode;
		pBody->pEnt = ebody;
		pBody->pAxis = pMNode;
		m_vBodies[i] = pBody;
	}

}

void ViconRTVisualizer::resetScene()
{
	// clean up all ogre related markers & bodies.

}

void ViconRTVisualizer::update(float addedTime)
{
	// update marker data
	m_pDataSet->update(addedTime);

	float pos[3], qt[4];
	if (m_bMarkerVisibility)
	{
		for (int i=0; i<m_iMarkers; i++)
		{
			m_pDataSet->getMarkerPosition(i, pos);
			m_vMarkerNodes[i]->setPosition(Ogre::Vector3(pos[0], pos[1], pos[2]));
		}
	}

	// update body data
	if (m_bBodyVisibility)
	{
		for (int i=0; i<m_iBodies; i++)
		{
			m_pDataSet->getBodyTranslation(i, pos);
			m_pDataSet->getBodyQuaternion(i, qt, false);

			m_vBodies[i]->pNode->setPosition(Ogre::Vector3(pos[0], pos[1], pos[2]));
			m_vBodies[i]->pNode->setOrientation(Quaternion(qt[0], qt[1], qt[2], qt[3]));
		}
	}

}
